package robocup.component.geometry;

import java.util.LinkedList;

import robocup.component.geometry.analysers.ReachableArea;

/**
 * @author Thomas Karbe
 * 
 */
public class CircleSegment {

  private final Vektor        center     = new Vektor();
  private double              radius     = 0;
  private double              startAngle = 0;
  private double              endAngle   = 0;

  /** ************************** DUMMIES ************************* */

  private final StraightLine  dummyLine  = StraightLine.getDummyInstance();

  /** ************************** STATICS ************************* */

  /**
   * "epsilon"
   */
  private final static double E          = 0.000001;

  /** ************************** CONSTRUCTORS ******************** */

  /**
   * Creates a dummy-instance of a CircleSegment.
   * 
   * <br>
   * NEW OBJECTS: 3 + this
   */
  public CircleSegment() {

  }

  /**
   * Creates a CircleSegment with given center and radius. Vektors are copied.
   * The Segment will be like a full Circle.
   * 
   * <br>
   * NEW OBJECTS: 3 + this
   * 
   * @param center
   *          the center point
   * @param radius
   *          the radius
   */
  public CircleSegment(
      final Vektor center,
      final double radius) {

    this.center.copy(center);
    this.radius = Math.abs(radius);
    startAngle = -180;
    endAngle = 180;
  }

  /**
   * Creates a CircleSegment with given center, radius, startAngle and endAngle.
   * Vektors are copied.
   * 
   * <br>
   * NEW OBJECTS: 3 + this
   * 
   * @param center
   *          the center point
   * @param radius
   *          the radius
   * @param startAngle
   * @param endAngle
   */
  public CircleSegment(
      final Vektor center,
      final double radius,
      final double startAngle,
      final double endAngle) {

    this.center.copy(center);
    this.radius = Math.abs(radius);
    this.startAngle = Vektor.normalize(startAngle);
    this.endAngle = Vektor.normalize(endAngle);
  }

  /** ************************** STATICS ************************* */

  /**
   * Creates a dummy-instance of a CircleSegment.
   * 
   * <br>
   * NEW OBJECTS: 4
   * 
   * @return a dummy instance
   */
  public static final CircleSegment getDummyInstance() {

    return new CircleSegment();
  }

  /** ************************** METHODS ************************* */

  /**
   * Copies the values from the given CircleSegment.
   * 
   * @param cs
   *          the CircleSegment to copy
   */
  public void copy(final CircleSegment cs) {

    center.copy(cs.center);
    radius = cs.radius;
    startAngle = cs.startAngle;
    endAngle = cs.endAngle;
  }

  /**
   * Returns a duplicate of this CircleSegment
   * 
   * <br>
   * NEW OBJECTS: 4
   * 
   * @return a duplicate of this object
   */
  public final CircleSegment cloned() {

    return new CircleSegment(center, radius, startAngle, endAngle);
  }

  /**
   * sets the CircleSegment to dummyValues
   */
  public void reset() {

    center.x = 0;
    center.y = 0;
    radius = 0;
    startAngle = 0;
    endAngle = 0;
  }

  /**
   * sets the endAngle
   * 
   * @param endAngle
   *          the Angle to set
   */
  public void setEndAngle(final double endAngle) {

    this.endAngle = Vektor.normalize(endAngle);
  }

  /**
   * sets the radius
   * 
   * @param radius
   *          the radius to set
   */
  public void setRadius(final double radius) {

    this.radius = radius;
  }

  /**
   * sets the startAngle
   * 
   * @param startAngle
   *          the startAngle to set
   */
  public void setStartAngle(final double startAngle) {

    this.startAngle = Vektor.normalize(startAngle);
  }

  /**
   * sets the center
   * 
   * @param pos
   *          the new center
   */
  public void copyCenter(final Vektor pos) {

    center.copy(pos);
  }

  /**
   * Returns the center
   * 
   * @return the center
   */
  public final Vektor getCenter() {

    return center;
  }

  /**
   * Returns the end angle
   * 
   * @return the end angle
   */
  public final double getEndAngle() {

    return endAngle;
  }

  /**
   * Returns the radius
   * 
   * @return the radius
   */
  public final double getRadius() {

    return radius;
  }

  /**
   * Returns the start angle
   * 
   * @return the start angle
   */
  public final double getStartAngle() {

    return startAngle;
  }

  /**
   * Returns a String representation of the CircleSegment
   * 
   * @see java.lang.Object#toString()
   */
  public final String toString() {

    StringBuffer sb = new StringBuffer();
    sb.append("CircleSegment: sA: ").append(startAngle).append(" eA: ").append(
        endAngle).append(" rad: ").append(radius).append(" center: ").append(
        center);
    return sb.toString();
  }

  /**
   * Calculates the point with the given angle and saves it to result. If no
   * such point exists, result will be set to empty.
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param alpha
   *          the angle to the point;
   * @param result
   *          old contents is overwritten, the needed point is saved here
   */
  public void getPosWithAngle(final double alpha,
      final Vektor result) {

    if (Vektor.isAngleBetween(startAngle, alpha, endAngle)) {
      result.setVektor(getRadius(), alpha);
      result.addToThis(center);
      return;
    }
    result.setEmpty();
  }

  /**
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param alpha
   *          the angle to the point;
   * @return the needed point
   */
  public final Vektor getPosWithAngle(final double alpha) {

    Vektor res = new Vektor();
    getPosWithAngle(alpha, res);
    return res.isEmpty() ? null : res;
  }

  /**
   * Calculates, if the CircleSegment includes the given angle.
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param angle
   * @return true if the given angle is included, false otherwise.
   */
  public final boolean includesAngle(final double angle) {

    return Vektor.isAngleBetween(startAngle, angle, endAngle);
  }

  /**
   * Calculates the degreedistance from startAngle to endAngle
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @return the angle between startAngle and endAngle
   */
  public final double getAngleDiff() {

    if (startAngle <= endAngle) {
      return endAngle - startAngle;
    }
    return 360 - startAngle + endAngle;
  }

  /**
   * Calculates the distance from startangle to the given angle.
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param angle
   *          the given angle
   * @return the distance in degree
   */
  public final double getDistToAngle(final double angle) {

    if (startAngle <= angle) {
      return angle - startAngle;
    }
    return 360 - startAngle + angle;
  }

  /**
   * Calculates all intersection points between this CircleSegment and the given
   * LineSegment.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param line
   *          the LineSegment to intersect with
   * @return a list of all intersection points (maybe empty)
   */
  public final LinkedList<Vektor> getIntersectionPoints(final LineSegment line) {

    LinkedList<Vektor> list = new LinkedList<Vektor>();
    getIntersectionPoints(line, list);
    return list;
  }

  /**
   * Calculates the intersection points between this CircleSegment and a
   * LineSegment and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param line
   *          the LineSegment to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final LineSegment line,
      final LinkedList<Vektor> list) {

    line.addIntersectionPointsToList(this, list);
  }

  /**
   * Calculates the intersection points between this CircleSegment and a
   * StraightLine and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param line
   *          the StraightLine to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final StraightLine line,
      final LinkedList<Vektor> list) {

    LinkedList<Vektor> list2 = getIntersectionPoints(line);
    for (Vektor v : list2) {
      if (v != null) {
        list.add(v);
      }
    }
  }

  /**
   * Calculates the intersection points between this CircleSegment and a Circle
   * and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES CircleSegment.dummyLine USES StraightLine.dummyVektor
   * 
   * @param circ
   *          the Circle to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final Circle circ,
      final LinkedList<Vektor> list) {

    LinkedList<Vektor> list2 = getIntersectionPoints(circ);
    for (Vektor v : list2) {
      if (v != null) {
        list.add(v);
      }
    }
  }

  /**
   * Calculates the intersection points between two CircleSegments and adds them
   * to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES CircleSegment.dummyLine
   * 
   * @param circ
   *          the CircleSegment to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final CircleSegment circ,
      final LinkedList<Vektor> list) {

    LinkedList<Vektor> list2 = getIntersectionPoints(circ);
    for (Vektor v : list2) {
      if (v != null) {
        list.add(v);
      }
    }
  }

  /**
   * Calculates all intersection points between this CircleSegment and the given
   * LineSegment.
   * 
   * <br>
   * NEW OBJECTS <= 2
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param line
   *          the LineSegment to intersect with
   * @param result
   *          old contents will be overwritten, intersectionpoints will be saved
   *          here
   */
  public void getIntersectionPoints(final LineSegment line,
      final LinkedList<Vektor> result) {

    if (!result.isEmpty()) {
      result.clear();
    }
    double xm = line.getPoint2().x - line.getPoint1().x;
    if (xm == 0) {
      xm += CircleSegment.E;
    }
    double ym = line.getPoint2().y - line.getPoint1().y;
    if (ym == 0) {
      ym += CircleSegment.E;
    }
    double xs = line.getPoint1().x;
    double ys = line.getPoint1().y;
    double xc = center.x;
    double yc = center.y;
    double base = -((xm * (xs - xc) + ym * (ys - yc)) / (xm * xm + ym * ym));
    double det = base
        * base
        - (((xs - xc) * (xs - xc) + (ys - yc) * (ys - yc) - radius * radius) / (xm
            * xm + ym * ym));
    if (det < 0) {
      return;
    }
    if (det == 0) {
      Vektor v = new Vektor();
      v.x = xs + base * xm;
      v.y = ys + base * ym;
      if (line.getPoint1().getDistance(v) > line.getLength()) {
        return;
      }
      if (line.getPoint2().getDistance(v) > line.getLength()) {
        return;
      }
      if (!this.includesAngle(center.getAngleTo(v))) {
        return;
      }
      result.add(v);
      return;
    }
    double s1 = base + Math.sqrt(det);
    double s2 = base - Math.sqrt(det);
    Vektor v1 = new Vektor();
    Vektor v2 = new Vektor();
    v1.x = xs + s1 * xm;
    v1.y = ys + s1 * ym;
    v2.x = xs + s2 * xm;
    v2.y = ys + s2 * ym;
    if (line.getPoint1().getDistance(v1) <= line.getLength()) {
      if (line.getPoint2().getDistance(v1) <= line.getLength()) {
        if (this.includesAngle(center.getAngleTo(v1))) {
          result.add(v1);
        }
      }
    }
    if (line.getPoint1().getDistance(v2) <= line.getLength()) {
      if (line.getPoint2().getDistance(v2) <= line.getLength()) {
        if (this.includesAngle(center.getAngleTo(v2))) {
          result.add(v2);
        }
      }
    }
    return;
  }

  /**
   * Calculates the intersection points between both CircleSegments
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES CircleSegment.dummyLine
   * 
   * @param c
   *          the CircleSegment to intersect with
   * @return a list with all intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final CircleSegment c) {

    LinkedList<Vektor> list = new LinkedList<Vektor>();
    getIntersectionPoints(c, list);
    return list;
  }

  /**
   * Calculates the intersection points between both CircleSegments
   * 
   * <br>
   * NEW OBJECTS <= 2 <br>
   * USES CircleSegment.dummyLine
   * 
   * @param c
   *          the CircleSegment to intersect with
   * @param result
   *          old contents will be overwritten. Here the results are saved.
   */
  public final void getIntersectionPoints(final CircleSegment c,
      final LinkedList<Vektor> result) {

    // System.out.println("\nINTERSECTION BETWEEN");
    // System.out.println("CS: "+this);
    // System.out.println("Circle: "+c+"\n");

    if (!result.isEmpty()) {
      result.clear();
    }
    final double dist = c.getCenter().getDistance(getCenter());
    if (dist > c.getRadius() + getRadius()) {
      return;
    }
    if (Math.max(getRadius(), c.getRadius()) - dist > Math.min(c.getRadius(),
        getRadius())) {
      return;
    }

    final double x1 = getCenter().x;
    final double x2 = c.getCenter().x;
    final double y1 = getCenter().y;
    final double y2 = c.getCenter().y;
    double dividor = y2 - y1;
    if (dividor == 0) {
      dividor += CircleSegment.E;
    }

    final double m = (x1 - x2) / dividor;
    final double n = (x2
        * x2
        - x1
        * x1
        + y2
        * y2
        - y1
        * y1
        + getRadius()
        * getRadius() - c.getRadius() * c.getRadius())
        / (2 * dividor);
    dummyLine.setStraightLine(m, n);
    // System.out.println("int line: "+dummyLine);
    dummyLine.getIntersectionPoints(this, result);
    for (int i = result.size() - 1; i >= 0; i--) {
      if (result.get(i) == null) {
        result.remove(i);
        continue;
      }
      if (!c.includesAngle(c.getCenter().getAngleTo(result.get(i)))) {
        result.remove(i);
      }
    }
  }

  /**
   * Calculates the intersection points between this CircleSegment and a Circle
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES CircleSegment.dummyLine USES StraightLine.dummyVektor
   * 
   * @param c
   *          the Circle to intersect with
   * @return a list with all intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final Circle c) {

    LinkedList<Vektor> list = new LinkedList<Vektor>();
    getIntersectionPoints(c, list);
    return list;
  }

  /**
   * Calculates the intersection points between this CircleSegment and a Circle
   * 
   * <br>
   * NEW OBJECTS <= 2 <br>
   * USES CircleSegment.dummyLine USES StraightLine.dummyVektor
   * 
   * @param c
   *          the Circle to intersect with
   * @param result
   *          old contents will be overwritten. Here the results are saved.
   */
  public final void getIntersectionPoints(final Circle c,
      final LinkedList<Vektor> result) {

    // System.out.println("\nINTERSECTION BETWEEN");
    // System.out.println("CS: "+this);
    // System.out.println("Circle: "+c+"\n");

    if (!result.isEmpty()) {
      result.clear();
    }
    final double dist = c.getCenter().getDistance(getCenter());
    if (dist > c.getRadius() + getRadius()) {
      return;
    }
    if (Math.max(getRadius(), c.getRadius()) - dist > Math.min(c.getRadius(),
        getRadius())) {
      return;
    }

    final double x1 = getCenter().x;
    final double x2 = c.getCenter().x;
    final double y1 = getCenter().y;
    final double y2 = c.getCenter().y;
    double dividor = y2 - y1;
    if (dividor == 0) {
      dividor += CircleSegment.E;
    }

    final double m = (x1 - x2) / dividor;
    final double n = (x2
        * x2
        - x1
        * x1
        + y2
        * y2
        - y1
        * y1
        + getRadius()
        * getRadius() - c.getRadius() * c.getRadius())
        / (2 * dividor);
    dummyLine.setStraightLine(m, n);
    // System.out.println("int line: "+dummyLine);
    dummyLine.getIntersectionPoints(this, result);
  }

  /**
   * Calculates the intersection points between this CircleSegment and a
   * StraightLine
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param line
   *          the SraightLine to intersect with
   * @return a list with all intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final StraightLine line) {

    return line.getIntersectionPoints(this);
  }

  /**
   * Calculates the intersection points between this CircleSegment and a
   * StraightLine
   * 
   * <br>
   * NEW OBJECTS <= 2
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param line
   *          the StraightLine to intersect with
   * @param result
   *          old contents will be overwritten. Here the results are saved.
   */
  public void getIntersectionPoints(final StraightLine line,
      final LinkedList<Vektor> result) {

    line.getIntersectionPoints(this, result);
  }
  
  /**
   * Calculates the intersection points of this CircleSegment and a RA and add the results to the given list.
   * 
   * @param ra the RA to intersect with
   * @param back if true, backwards movement is allowed
   * @return a list that contains the intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final ReachableArea ra, final boolean back) {
    return ra.getIntersectionPoints(this, back);
  }
  
  /**
   * Calculates the intersection points of this CircleSegment and a RA and add the results to the given list.
   * 
   * @param ra the RA to intersect with
   * @param back if true, backwards movement is allowed
   * @param list here the results are added
   */
  public final void addIntersectionPointsToList(final ReachableArea ra, final boolean back, final LinkedList<Vektor> list) {
    ra.addIntersectionPointsToList(this, back, list);
  }

  /**
   * <br>
   * NEW OBJECTS: 4
   * 
   * @return a circle that covers this CircleSegment
   */
  public final Circle getCircle() {

    return new Circle(center, radius);
  }

  /**
   * @param result
   *          old contents will be deleted. The new contents will be the Circle
   *          that covers this CircleSegment.
   */
  public void getCircle(final Circle result) {

    result.setCenter(center);
    result.setRadius(radius);
  }
}
